/**
  ******************************************************************************
  * @file    test_matplot.cpp
  * @author  Mentos Seetoo
  * @brief   This file provides examples to use matplotlib-cpp and test whether
  *          this library can work well in your enviroment.		
  * @warning 
  *          - At least C++11 is required.														 
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include <vector>
#include "test.h"

using namespace std;
namespace plt = matplotlibcpp;

/* Functions -----------------------------------------------------------------*/
void test_matplot()
{
    /* 2D-Plot ---------------------------------------------------*/
    // Prepare data.
    int n = 5000; // number of data points
    vector<double> x(n),y(n);
    for(int i=0; i<n; ++i) {
        double t = 2*M_PI*i/n;
        x.at(i) = 16*sin(t)*sin(t)*sin(t);
        y.at(i) = 13*cos(t) - 5*cos(2*t) - 2*cos(3*t) - cos(4*t);
    } 
    // [Optional] Set the size of output image
    plt::figure_size(1024, 768);
    // [Optional] Add graph title
    plt::title("Sample figure");
    /* 
       plot() takes an arbitrary number of (x,y,format)-triples.
       x must be iterable (that is, anything providing begin(x) and end(x)),
       y must either be callable (providing operator() const) or iterable.
    */
    plt::named_plot("Example", x, y, "r-");
    plt::plot(x, [](double d){ return -2.5+abs(sin(d));}, "g-");
    // [Optional] Enable legend.
    plt::legend();
    // show plots
    //plt::show();
    // [Optional] Save image.
    //plt::save("./robot/cache/Matplot_Example.jpg");

    /* 3D-Plot ---------------------------------------------------*/
    std::vector<std::vector<double>> x2, y2, z2;
    for (double i = -5; i <= 5;  i += 0.25) {
        std::vector<double> x_row, y_row, z_row;
        for (double j = -5; j <= 5; j += 0.25) {
            x_row.push_back(i);
            y_row.push_back(j);
            z_row.push_back(::std::sin(::std::hypot(i, j)));
        }
        x2.push_back(x_row);
        y2.push_back(y_row);
        z2.push_back(z_row);
    }
    plt::plot_surface(x2, y2, z2);
    plt::title("3D Surface");
    plt::legend();

    std::vector<double> x3,y3,z3;
    double theta, r;
    double z_inc = 4.0/99.0; double theta_inc = (8.0 * M_PI)/99.0;
    for (double i = 0; i < 100; i += 1) {
        theta = -4.0 * M_PI + theta_inc*i;
        z3.push_back(-2.0 + z_inc*i);
        r = z3[i]*z3[i] + 1;
        x3.push_back(r * sin(theta));
        y3.push_back(r * cos(theta));
    }
    std::map<std::string, std::string> keywords;
    keywords.insert(std::pair<std::string, std::string>("label", "parametric curve") );
    keywords.insert(std::pair<std::string, std::string>("linewidth", "2") );
    keywords.insert(std::pair<std::string, std::string>("color", "r") );

    plt::plot3(x3, y3, z3, 1, keywords);
    plt::title("3D Curve");
    plt::legend();
    std::cout << "[Unit Test] If you can see figure windows, Matplotlib-cpp test is passed, and You can close the figure windows." <<endl; 
    std::cout << "[Unit Test] If there is no window, please check your enviroment." << endl;
    plt::show();
}

void test_plotTraj(int dim, float x, float y, float z)
{
    static float x2[2] = {0},y2[2] = {0};
    static std::vector<float> x1,y1,z1;
    if(dim == 2)
    {
        x2[0] = x2[1];
        y2[0] = y2[1];
        x2[1] = x;
        y2[1] = y;

        plt::plot(x2,y2,"b-");
        plt::pause(0.001);
        plt::title("Figure: trajectory");
        plt::legend();
    }
    else if(dim ==3)
    {
        x1.push_back(x);
        y1.push_back(y);
        z1.push_back(z);
        plt::plot3(x1, y1, z1, 1);
        plt::title("Foot trajectory");
        plt::pause(0.001);
    }
}
/************************* END-OF-FILE SCUT-ROBOTLAB **************************/